%读地图  画图
clc;
clear;
format long;

filename = 'mapclear.csv' ;
MapPoint=csvread(filename);   %三列分别为 点类型 X Y

[m,n]=size(MapPoint);

cmap=[0.75 0.75 0.75;  % 1 灰色 自由通行
    0    1    0; % 2 绿色 储位节点
    1    1    0;% 3 黄色 保留节点
    0    0    0;% 4 黑色 柱子节点
    0    0    1;% 5 蓝色 拣选工位节点
    1    0    1;% 6 粉色 补货位节点
    1    0    0;% 7 红色 空托盘回收节点
    ];
colormap(cmap);

Color=ones(23,33);  %pcolor从1开始画，这里行列+1
for i=1:1:m
    Color(MapPoint(i,3)+1,MapPoint(i,2)+1)= MapPoint(i,1);  %MapPoint从0开始计数，此处+1
end
for i=1:1:m
    MapPoint(i,3)=MapPoint(i,3)+1;
    MapPoint(i,2)=MapPoint(i,2)+1;
end

pcolor(Color);

fid = fopen('mapclear2.csv');   %打开并获取csv文件
% [A,COUNT]=fscanf(fid,'%s',inf);
Map2=100*ones(704,8);
for i=1:1:703
    A1=fgetl(fid);
    num_str = regexp(A1,'\d*\.?\d*','match');
    [M,N]=size(num_str);
    for j=1:1:N
        Map2(i,j)=str2double(num_str(1,j));
    end
end
fclose(fid);

%全部+1
for i=1:1:704
    for j=1:1:8
        if Map2(i,j)~=100
            Map2(i,j)=Map2(i,j)+1;
        end
    end
end

MapData=zeros(704,12);
for i=1:1:704
    MapData(i,1:3)=MapPoint(i,1:3);
    MapData(i,5:12)=Map2(i,1:8);
    count=0;
    for j=1:1:8
        if Map2(i,j)~=100
            count=count+1;
        end
    end
    MapData(i,4)=count/2;
end

save('MapData.mat','MapData');
